/*
* @File Name fK.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\fK\fK.c
* @Author: Ruige_Lee
* @Date:   2019-08-20 19:11:22
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-08-20 20:49:10
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/

#include <stdint.h>

#include "fK.h"
#include "math/fast_math.h"


//只需要关注6个节点，4个关节（4，5合一个，6，tool合一个）
int32_t fK_nomal_solve(float robotPose[6], float LinkDot[5][3])
{
	float s1 = fast_sin(robotPose[0]);
	float s2 = fast_sin(robotPose[1]);
	float s3 = fast_sin(robotPose[2]);
	float s4 = fast_sin(robotPose[3]);
	float s5 = fast_sin(robotPose[4]);
	float s6 = fast_sin(robotPose[5]);

	float c1 = fast_cos(robotPose[0]);
	float c2 = fast_cos(robotPose[1]);
	float c3 = fast_cos(robotPose[2]);
	float c4 = fast_cos(robotPose[3]);
	float c5 = fast_cos(robotPose[4]);
	float c6 = fast_cos(robotPose[5]);

	LinkDot[0][0] = 0.0;
	LinkDot[0][1] = 0.0;
	LinkDot[0][2] = 0.0;

	LinkDot[1][0] = 0.0;
	LinkDot[1][1] = 0.0;
	LinkDot[1][2] = 264;

	LinkDot[2][0] = -225*c1*s2;
	LinkDot[2][1] = -225*s1*s2;
	LinkDot[2][2] = 225*c2 + 264;

	LinkDot[3][0] = (217.3*c1*c2*c3) - 225*c1*s2 - (217.3*c1*s2*s3);
	LinkDot[3][1] = (217.3*c2*c3*s1) - 225*s1*s2 - (217.3*s1*s2*s3);
	LinkDot[3][2] = 225*c2 + (217.3*c2*s3) + (217.3*c3*s2) + 264;

	LinkDot[4][0] = (217.3*c1*c2*c3) - 40*s5*(c1*c2*c3 - c1*s2*s3) - 40*c5*(s1*s4 - c4*(c1*c2*s3 + c1*c3*s2)) - 225*c1*s2 - (217.3*c1*s2*s3);
	LinkDot[4][1] = 40*c5*(c1*s4 + c4*(c2*s1*s3 + c3*s1*s2)) - 40*s5*(c2*c3*s1 - s1*s2*s3) - 225*s1*s2 + (217.3*c2*c3*s1) - (217.3*s1*s2*s3);
	LinkDot[4][2] = 225*c2 + (217.3*c2*s3) + (217.3*c3*s2) - 40*s5*(c2*s3 + c3*s2) - 40*c4*c5*(c2*c3 - s2*s3) + 264;

	return 0;
}

int32_t fK_conversion_solve(float robotPose[6], float LinkDot[5][3])
{
	float s1 = fast_sin(robotPose[0]);
	float s2 = fast_sin(robotPose[1]);
	float s3 = fast_sin(robotPose[2]);
	float s4 = fast_sin(robotPose[3]);
	float s5 = fast_sin(robotPose[4]);
	float s6 = fast_sin(robotPose[5]);

	float c1 = fast_cos(robotPose[0]);
	float c2 = fast_cos(robotPose[1]);
	float c3 = fast_cos(robotPose[2]);
	float c4 = fast_cos(robotPose[3]);
	float c5 = fast_cos(robotPose[4]);
	float c6 = fast_cos(robotPose[5]);

	LinkDot[0][0] = 650.0;
	LinkDot[0][1] = 0.0;
	LinkDot[0][2] = 0.0;

	LinkDot[1][0] = 650.0;
	LinkDot[1][1] = 0.0;
	LinkDot[1][2] = 264;

	LinkDot[2][0] = 225*c1*s2 + 650;
	LinkDot[2][1] = 225*s1*s2;
	LinkDot[2][2] = 225*c2 + 264;

	LinkDot[3][0] = 225*c1*s2 - (217.3*c1*c2*c3) + (217.3*c1*s2*s3) + 650;
	LinkDot[3][1] = 225*s1*s2 - (217.3*c2*c3*s1) + (217.3*s1*s2*s3);
	LinkDot[3][2] = 225*c2 + (217.3*c2*s3) + (217.3*c3*s2) + 264;

	LinkDot[4][0] = 225*c1*s2 + 40*s5*(c1*c2*c3 - c1*s2*s3) + 40*c5*(s1*s4 - c4*(c1*c2*s3 + c1*c3*s2)) - (217.3*c1*c2*c3) + (217.3*c1*s2*s3) + 650;
	LinkDot[4][1] = 225*s1*s2 + 40*s5*(c2*c3*s1 - s1*s2*s3) - 40*c5*(c1*s4 + c4*(c2*s1*s3 + c3*s1*s2)) - (217.3*c2*c3*s1) + (217.3*s1*s2*s3);
	LinkDot[4][2] = 225*c2 + (217.3*c2*s3) + (217.3*c3*s2) - 40*s5*(c2*s3 + c3*s2) - 40*c4*c5*(c2*c3 - s2*s3) + 264;


	return 0;
}



